#define CTRL_C 3
#define CTRL_X 24

void gotoCL()
{
  digitalWrite(35,HIGH);
  digitalWrite(36,LOW);
  digitalWrite(37,LOW);
  Serial.println("-----DROPPED TO A SHELL-----");
  bool run = true;
  char buffer[50];
  int head = 0;
  while (run)
  {
	Serial.print(">>");
	while (head==0 || buffer[head-1]!='\n')
	{
	  if (Serial.available())
	  {
		char c = Serial.read();
		Serial.print(c);
		if (c == '\b')
		{
		  if (head > 0)
		  {
			head--;
			buffer[head] = '\0';
		  }
		  else {Serial.print(">");}
		}
		else
		{
		  buffer[head] = c;
		  head++;
		  buffer[head] = '\0';
		}
	  }
	}
	if (buffer[0]=='\r' || buffer[0]=='\n'){}
	else if (helpMenu(buffer) || cmdsFlash(buffer) || cmdsPid(buffer) || cmdsServo(buffer) || cmdsAP(buffer) || cmdsGps(buffer) || cmdsReg(buffer)){}
	else if (chk(buffer,"exit")){run = false;}
	else if (chk(buffer,"char")){Serial.println(int(buffer[5]));}
	else
	{
	  Serial.print("Unknown command: ");
	  Serial.println(buffer);
	}
	head = 0;
  }
  digitalWrite(35,LOW);
  digitalWrite(36,LOW);
  digitalWrite(37,HIGH);
}

bool helpMenu(char *buffer)
{
  if (!chk(buffer,"help")){return false;}
  Serial.println("Available commands:");
  Serial.println("1. help -> This menu system");
  Serial.println("2. fl   -> Flash Memory");
  Serial.println("3. pid  -> PID");
  Serial.println("4. svo  -> Manual Servo Testing");
  Serial.println("5. exit -> Close the shell");
  Serial.println("6. gps  -> Show the gps status & info");
  Serial.println();
  Serial.println("For more info on each command, type <command> help");
  return true;
}

bool cmdsFlash(char *buffer)
{
  if (!chk(buffer,"fl")){return false;}
  char *arg = next(buffer);
  if (chk(arg,"help"))
  {
	Serial.println("-----------------------");
	Serial.println("-----Flash Command-----");
	Serial.println("-----------------------");
	Serial.println("--Help menu:");
	Serial.println();
	Serial.println("manu ---------- Print the Manufacturer ID");
	Serial.println("read ---------- Read a specific page");
	Serial.println("    1 : Page number");
	Serial.println("    2 : Size");
	Serial.println("    3 : Type");
	Serial.println("write --------- Write to a specific page");
	Serial.println("    1 : Page number");
	Serial.println("    2 : Contents");
	Serial.println("    3 : Type");
	Serial.println();
	Serial.println("Types: b -> bytes, c -> charecters, h -> hex");
	Serial.println();
	Serial.println();
  }
  else if (chk(arg,"manu"))
  {
	flash.ReadManufacturerID();
	Serial.print("Manufacturer ID : ");
	Serial.print(int(flash.df_manufacturer));
	Serial.print(", ");
	Serial.println(flash.df_device);
  }
  else if (chk(arg,"read"))
  {
	char *arg2 = next(arg);
	char *arg3 = next(arg2);
	char arg4 = *next(arg3);
	int page = parseNumberArg(arg2);
	int size = parseNumberArg(arg3);
	if (page == -1){Serial.println("Page argument (arg 2) is not a valid integer");}
	else if (size == -1){Serial.println("Page argument (arg 3) is not a valid integer");}
	else if (size == 0){Serial.println("Cannot read a size of 0");}
	else if (arg4 != 'b' && arg4 != 'c' && arg4 != 'h'){Serial.print("Unknown type for argument type (arg 4)");}
	else
	{
	  flash.StartRead(page);
	  for (int i=0;i<size;i++)
	  {
		if (arg4 == 'b')
		{
		  Serial.print(int(flash.ReadByte()));
		  Serial.print(", ");
		}
		else if (arg4 == 'c')
		{
		  Serial.print(char(flash.ReadByte()));
		}
		else
		{
		  int b,bl,bh;
		  b = flash.ReadByte();
		  bl = b&0x0F;
		  bh = (b&0xF0)>>4;
		  Serial.print(inhex(bh));
		  Serial.print(inhex(bl));
		  Serial.print(", ");
		}
	  }
	  Serial.println();
	}
  }
  else if (chk(arg,"write"))
  {
	char *arg2 = next(arg);
	char *arg3 = next(arg2);
	char arg4 = *next(arg3);
	int page = parseNumberArg(arg2);
	if (page == -1){Serial.println("Page argument (arg 2) is not a valid integer");}
	else if (arg4 != 'i' && arg4 != 'c' && arg4 != 'h'){Serial.print("Unknown type for argument type (arg 4)");}
	else
	{
	  flash.StartWrite(page);
	  int i;
	  if (arg4 == 'c')
	  {
		for (i = 0;!blank(arg3[i]);i++)
		{
		  if (arg3[i]=='\\')
		  {
			i++;
			if (arg3[i]=='\\'){flash.WriteByte('\\');}
			else if (arg3[i]=='_'){flash.WriteByte(' ');}
			else if (arg3[i]=='t'){flash.WriteByte('\t');}
			else if (arg3[i]=='0'){flash.WriteByte('\0');}
			else if (arg3[i]=='r'){flash.WriteByte('\r');}
			else if (arg3[i]=='n'){flash.WriteByte('\n');}
			else if (arg3[i]=='b'){flash.WriteByte('\b');}
			else {flash.WriteByte('?');}
		  }
		  else{flash.WriteByte(arg3[i]);}
		}
	  }
	  else
	  {
		Serial.println("NOT MADE YET");
	  }
	}
  }
  else
  {
	Serial.println("Unrecognised argument, try using \"fl help\" for more details.");
  }
  return true;
}

bool cmdsPid(char *buffer)
{
  if (!chk(buffer,"pid")){return false;}
  char *arg1 = next(buffer);
  if (chk(arg1,"help"))
  {
	Serial.println("----------------------");
	Serial.println("-----PID Commands-----");
	Serial.println("----------------------");
	Serial.println("--Help menu:");
	Serial.println();
	Serial.println("ls ------- List pids");
	Serial.println("    [1] : PID name* or all to list all");
	Serial.println("        Optional argument: leave out for all");
	Serial.println("w -------- Ammend a pid preference");
	Serial.println("    1 : PID name*");
	Serial.println("    2 : Attribute title");
	Serial.println("    3 : Value");
	Serial.println("    [4] : 2nd value (only used for attribute title \"pid\")");
	Serial.println("    [5] : 3rd value (only used for attribute title \"pid\")");
	Serial.println();
	Serial.println("    Valid attribute titles:");
	Serial.println("        p, i, d, max, min, inv");
	Serial.println();
	Serial.println("    Valid values:");
	Serial.println("        for p, i, d, max, min: type double");
	Serial.println("        for max, min: off, -");
	Serial.println("        for inv: on, off, inv");
	Serial.println("        for pid: three type doubles as arguments 3,4,5");
	Serial.println("clear ---- Clear PIDs");
	Serial.println("    p,i,d are reset to 0.0");
	Serial.println("    max and min are disabled");
	Serial.println("    inverting is disabled");
	Serial.println("    Accumulator is left as is");
	Serial.println();
	Serial.println("    1 : PID name* (or leave blank or \"all\" for all pids)");
	Serial.println("        If attempting to clear all PIDs, the user will be prompted");
	Serial.println("        y/n before the action comenses. typing in \"n\" will cancel");
	Serial.println("        performing the wipe.");
	Serial.println();
	Serial.println("Valid PID names:");
	Serial.println("    Roll -> roll");
	Serial.println("    Heading -> heading, hdg");
	Serial.println("    Pitch -> pitch");
	Serial.println("    V-Rate -> vrate");
	Serial.println("    Altitude -> alt");
	Serial.println("    Throttle -> th, thr, thro, throttle");
	Serial.println("    Slip -> slip");
  }
  else if (chk(arg1,"ls"))
  {
	char *arg2 = next(arg1);
	if (chk(arg2,"") || chk(arg2,"all"))
	{
	  Serial.println("Listing all PIDs:");
	  dispPid("1.Roll --------",&aircraft.ap.pid.rollHold);
	  dispPid("2.  Heading ---",&aircraft.ap.pid.headingHold);
	  dispPid("3.Pitch -------",&aircraft.ap.pid.pitchHold);
	  dispPid("4.  V-Rate ----",&aircraft.ap.pid.vrateHold);
	  dispPid("5.    Altitude ",&aircraft.ap.pid.altHold);
	  dispPid("6.Throttle ----",&aircraft.ap.pid.throttleHold);
	  dispPid("7.Slip --------",&aircraft.ap.pid.slipHold);
	}
	else if (chk(arg2,"roll")){dispPid("Roll",&aircraft.ap.pid.rollHold);}
	else if (chk(arg2,"heading") || chk(arg2,"hdg")){dispPid("Heading",&aircraft.ap.pid.headingHold);}
	else if (chk(arg2,"pitch")){dispPid("Pitch",&aircraft.ap.pid.pitchHold);}
	else if (chk(arg2,"vrate")){dispPid("V-Rate",&aircraft.ap.pid.vrateHold);}
	else if (chk(arg2,"alt")){dispPid("Altitude",&aircraft.ap.pid.altHold);}
	else if (chk(arg2,"th")||chk(arg2,"thr")||chk(arg2,"thro")||chk(arg2,"throttle")){dispPid("Throttle",&aircraft.ap.pid.throttleHold);}
	else if (chk(arg2,"slip")){dispPid("Slip",&aircraft.ap.pid.slipHold);}
	else
	{
	  Serial.print("Unknown argument");
	}
  }
  else if (chk(arg1,"w"))
  {
	char *arg2 = next(arg1);//name
	char *arg3 = next(arg2);//attr
	char *arg4 = next(arg3);//val
	double val = 0.0;
	bool onoff = false,changed = true;
	if (chk(arg3,"inv"))
	{
	  onoff = !(chk(arg4,"off") || chk(arg4,"-"));
	}
	else if (chk(arg3,"max") || chk(arg3,"min"))
	{
	  if (chk(arg4,"off") || chk(arg4,"-"))
	  {
		onoff = false;
	  }
	  else
	  {
		onoff = true;
		val = parseDoubleArg(arg4);
	  }
	}
	else
	{
	  val = parseDoubleArg(arg4);
	}
	PID *pid;
	if (chk(arg2,"roll")){pid = &aircraft.ap.pid.rollHold;}
	else if (chk(arg2,"heading") || chk(arg2,"hdg")){pid = &aircraft.ap.pid.headingHold;}
	else if (chk(arg2,"pitch")){pid = &aircraft.ap.pid.pitchHold;}
	else if (chk(arg2,"vrate")){pid = &aircraft.ap.pid.vrateHold;}
	else if (chk(arg2,"alt")){pid = &aircraft.ap.pid.altHold;}
	else if (chk(arg2,"th")||chk(arg2,"thr")||chk(arg2,"thro")||chk(arg2,"throttle")){pid = &aircraft.ap.pid.throttleHold;}
	else if (chk(arg2,"slip")){pid = &aircraft.ap.pid.slipHold;}
	if (chk(arg3,"p")){pid->setPGain(val);}
	else if (chk(arg3,"i")){pid->setIGain(val);}
	else if (chk(arg3,"d")){pid->setDGain(val);}
	else if (chk(arg3,"pid"))
	{
	  double vali,vald;
	  char *arg5,*arg6;
	  arg5 = next(arg4);
	  arg6 = next(arg5);
	  vali = parseDoubleArg(arg5);
	  vald = parseDoubleArg(arg6);
	  pid->setPIDGain(val,vali,vald);
	}
	else if (chk(arg3,"max"))
	{
	  if (onoff){pid->setLimitMax(val);}
	  else{pid->disableMaxLimit();}
	}
	else if (chk(arg3,"min"))
	{
	  if (onoff){pid->setLimitMin(val);}
	  else{pid->disableMinLimit();}
	}
	else if (chk(arg3,"inv"))
	{
	  pid->setInverted(onoff);
	}
	else
	{
	  changed = false;
	  Serial.println("unknown argument3.");
	}
	if (changed)
	{
	  aircraft.ap.savePIDtoEEPROM();
	}
  }
  else if (chk(arg1,"clear"))
  {
	char *arg2 = next(arg1);
	bool changed = true;
	if (chk(arg2,"") || chk(arg2,"all"))
	{
	  Serial.println("Are you sure you want to clear all PIDs? (y/n) > ");
	  while (Serial.available()==0);//wait
	  if ((Serial.read()|32) == 'y')
	  {
		aircraft.ap.pid.rollHold.clear();
		aircraft.ap.pid.headingHold.clear();
		aircraft.ap.pid.pitchHold.clear();
		aircraft.ap.pid.vrateHold.clear();
		aircraft.ap.pid.altHold.clear();
		aircraft.ap.pid.throttleHold.clear();
		aircraft.ap.pid.slipHold.clear();
	  }
	  else
	  {
		Serial.println("Canceled");
		changed = false;
	  }
	}
	else if (chk(arg2,"roll")){aircraft.ap.pid.rollHold.clear();}
	else if (chk(arg2,"heading") || chk(arg2,"hdg")){aircraft.ap.pid.headingHold.clear();}
	else if (chk(arg2,"pitch")){aircraft.ap.pid.pitchHold.clear();}
	else if (chk(arg2,"vrate")){aircraft.ap.pid.vrateHold.clear();}
	else if (chk(arg2,"alt")){aircraft.ap.pid.altHold.clear();}
	else if (chk(arg2,"th")||chk(arg2,"thr")||chk(arg2,"thro")||chk(arg2,"throttle")){aircraft.ap.pid.throttleHold.clear();}
	else if (chk(arg2,"slip")){aircraft.ap.pid.slipHold.clear();}
	else
	{
	  changed = false;
	  Serial.println("Unknown PID name");
	}
	if (changed)
	{
	  aircraft.ap.savePIDtoEEPROM();
	  Serial.println("Erased!");
	}
  }
  else
  {
	Serial.println("Unknown argument");
  }
  return true;
}

bool cmdsServo(char *buffer)
{
  if (!chk(buffer,"svo")){return false;}
  char *arg1 = next(buffer);
  if (chk(arg1,"help"))
  {
	Serial.println("------------------------");
	Serial.println("-----Servo Commands-----");
	Serial.println("------------------------");
	Serial.println("--Help menu:");
	Serial.println();
	Serial.println("test ---------- Tests movement on all servos");
	Serial.println("right --------- Tests right deflection (ailerons)");
	Serial.println("left ---------- Tests left deflection (ailerons)");
	Serial.println("up ------------ Tests up elevator");
	Serial.println("down ---------- Tests down elevator");
	Serial.println("rudder left --- Tests rudder left");
	Serial.println("rudder right -- Tests rudder right");
	Serial.println("center -------- Centers all surfaces");
	Serial.println("invert, inv --- Inverts a channel");
	Serial.println("    1 : Channel number or Servo name *");
	Serial.println("    [2] : on/off (optional, leave blank for toggle)");
	Serial.println("ep ------------ Set endpoints");
	Serial.println("    [1] : Channel number or Servo name *");
	Serial.println("        (leave arg1 blank to list all)");
	Serial.println("    [2] :");
	Serial.println("        * (int) min endpoint nanoseconds");
	Serial.println("        * \"auto\" for auto set via controller");
	Serial.println("        * <blank> or \"read\" to read the values");
	Serial.println("    [3] : max endpoint (only applicable if min endpoint is being defined)");
	Serial.println();
	Serial.println("Else:");
	Serial.println("    1 : Channel number or Servo name*");
	Serial.println("    2 : Position in nanoseconds");
	Serial.println();
	Serial.println("Valid Servo names:");
	Serial.println("    roll -> 1 (ailerons)");
	Serial.println("    pitch -> 2 (elevator)");
	Serial.println("    th, thr, thro, throttle -> 3 (throttle)");
	Serial.println("    rudder, yaw -> 4 (rudder)");
  }
  else if (chk(arg1,"test"))
  {
	aircraft.servoTest();
  }
  else if (chk(arg1,"right"))
  {
	aircraft.svoRoll(100);
	aircraft.updateRCout();
  }
  else if (chk(arg1,"left"))
  {
	aircraft.svoRoll(-100);
	aircraft.updateRCout();
  }
  else if (chk(arg1,"up"))
  {
	aircraft.svoPitch(100);
	aircraft.updateRCout();
  }
  else if (chk(arg1,"down"))
  {
	aircraft.svoPitch(100);
	aircraft.updateRCout();
  }
  else if (chk(arg1,"rudder"))
  {
	char *arg2 = next(arg1);
	if (chk(arg2,"left"))
	{
	  aircraft.svoRudder(-100);
	  aircraft.updateRCout();
	}
	else if (chk(arg2,"right"))
	{
	  aircraft.svoRudder(-100);
	  aircraft.updateRCout();
	}
	else
	{
	  Serial.println("Invalid rudder command");
	}
  }
  else if (chk(arg1,"center"))
  {
	aircraft.svoRoll(0);
	aircraft.svoPitch(0);
	aircraft.svoRudder(0);
	aircraft.updateRCout();
  }
  else if (chk(arg1,"inv") || chk(arg1,"invert"))
  {
	char *arg2 = next(arg1);
	char *arg3 = next(arg2);
	int ch = charToChannel(arg2);
	if (ch==-1){Serial.println("Invalid channel");}
	else if (chk(arg3,""))
	{
	  aircraft.svoInvert(ch);
	}
	else if (chk(arg3,"+") || chk(arg3,"on"))
	{
	  aircraft.svoInvert(ch,true);
	}
	else if (chk(arg3,"-") || chk(arg3,"off"))
	{
	  aircraft.svoInvert(ch,false);
	}
	else
	{
	  Serial.println("Unknown argument 3");
	}
  }
  else if (chk(arg1,"ep"))
  {
	char *arg2 = next(arg1);
	if (chk(arg2,""))
	{
	  //show all
	  Serial.println("-----ALL ENDPOINTS-----");
	  dispEP(1);
	  dispEP(2);
	  dispEP(3);
	  dispEP(4);
	}
	else
	{
	  int ch = charToChannel(arg2);
	  if (ch < 1 || ch > 4){Serial.println("Invalid channel");}
	  else
	  {
		char *arg3 = next(arg2);
		if (chk(arg3,"") || chk(arg3,"read"))
		{
		  dispEP(ch);
		}
		else if (chk(arg3,"auto"))
		{
		  Serial.print("NOT MADE YET!");
		}
		else
		{
		  int minv,maxv;
		  char *arg4 = next(arg3);
		  minv = parseNumberArg(arg3);
		  maxv = parseNumberArg(arg4);
		  if (minv == -1 || maxv == -1){Serial.println("Arguments 3 and 4 need to be and integer");}
		  else{aircraft.setSvoRange(ch,minv,maxv);}
		}
	  }
	}
  }
  else
  {
	char *arg2 = next(arg1);
	char *arg3 = next(arg2);
	int ch,val;
	bool neg=false;
	ch = charToChannel(arg2);
	if (ch != -1)
	{
	  if (arg3[0]=='-'){neg=true;arg3++;}
	  val = parseNumberArg(arg3);
	  if (val==-1){Serial.print("Undefined argument 3, needs to be an integer");}
	  else
	  {
		if (neg){val*=-1;}
		aircraft.RCout(ch,val);
		aircraft.updateRCout();
	  }
	}
	else
	{
	  Serial.println("Undefined argument 1");
	}
  }
  return true;
}

bool cmdsAP(char *buffer)
{
  if (!chk(buffer,"ap")){return false;}
  char *arg1 = next(buffer);
  if (chk(arg1,"help"))
  {
	Serial.println("----------------------------");
	Serial.println("-----AutoPilot Commands-----");
	Serial.println("----------------------------");
	Serial.println("--Help menu:");
	Serial.println();
	Serial.println("mode ---- Prints out the mode the autopilot is in");
	Serial.println("setup --- Runs a setup that configures the propper holds");
	Serial.println();
  }
  else if (chk(arg1,"mode"))
  {
	Serial.print("Mode = ");
	switch (aircraft.ap.getMode())
	{
	  case 0:Serial.println("Standby");break;
	  case 1:Serial.println("WP");break;
	  case 2:Serial.println("Indv");break;
	  case 3:Serial.println("Land");break;
	  default:Serial.println("Unknown");break;
	}
  }
  else if (chk(arg1,"setup"))
  {
	bool gotit=false,mode = false;
	char got;
	while (!gotit)
	{
	  Serial.println("General Mode: 1 individual holds ~ 2 waypoint\n  >>");
	  while (Serial.available()==0);
	  got = Serial.read();
	  gotit = true;
	  if (got=='1'){mode = false;}
	  else if (got=='2'){mode = true;}
	  else if (got==CTRL_C){return true;}
	  else{gotit=false;}
	}
	if (mode)
	{
	  Serial.println("NOT MADE YET!");
	}
	else
	{
	  gotit=false;
	  while (!gotit)
	  {
		Serial.println("Roll Channel: 0 off ~ 1 roll ~ 2 heading\n  >>");
		while (Serial.available()==0);
		got = Serial.read();
		gotit=true;
		if (got=='0'){aircraft.ap.indv_rollOff();}
		else if (got=='1'){aircraft.ap.indv_roll();}
		else if (got=='2'){aircraft.ap.indv_heading();}
		else if (got==CTRL_C){return true;}
		else{gotit=false;}
	  }
	  gotit=false;
	  while (!gotit)
	  {
		Serial.println("Pitch Channel: 0 off ~ 1 pitch ~ 2 vrate ~ 3 altitude\n  >>");
		while (Serial.available()==0);
		got = Serial.read();
		gotit=true;
		if (got=='0'){aircraft.ap.indv_pitchOff();}
		else if (got=='1'){aircraft.ap.indv_pitch();}
		else if (got=='2'){aircraft.ap.indv_vrate();}
		else if (got=='3'){aircraft.ap.indv_alt();}
		else if (got==CTRL_C){return true;}
		else{gotit=false;}
	  }
	  gotit=false;
	  while (!gotit)
	  {
		Serial.println("Throttle Channel: 0 off ~ 1 on\n  >>");
		while (Serial.available()==0);
		got = Serial.read();
		gotit=true;
		if (got=='0'){aircraft.ap.indv_throOff();}
		else if (got=='1'){aircraft.ap.indv_thro();}
		else if (got==CTRL_C){return true;}
		else{gotit=false;}
	  }
	  gotit=false;
	  while (!gotit)
	  {
		Serial.println("Slip Channel: 0 off ~ 1 on\n  >>");
		while (Serial.available()==0);
		got = Serial.read();
		gotit=true;
		if (got=='0'){aircraft.ap.indv_slipOff();}
		else if (got=='1'){aircraft.ap.indv_slip();}
		else if (got==CTRL_C){return true;}
		else{gotit=false;}
	  }
	}
  }
  return true;
}

bool cmdsGps(char *buffer)
{
  if (!chk(buffer,"gps")){return false;}
  Serial.println("----------------------");
  Serial.println("----- GPS Status -----");
  Serial.println("----------------------");
  Serial.println();
  Serial.print("Status: ");
  MyGPS &gps = *(aircraft.sensors.gps);
  if (gps.status()==GPS::NO_GPS){Serial.println("NO GPS");}
  else if (gps.status()==GPS::NO_FIX){Serial.println("NO FIX");}
  else {Serial.println("GOOD");}
  Serial.print("Latitude: ");Serial.println(gps.my_lat);
  Serial.print("Longitude: ");Serial.println(gps.my_lon);
  Serial.print("Altitude: ");Serial.println(gps.my_alt);
  Serial.print("Speed: ");Serial.println(gps.my_speed);
  Serial.print("Course: ");Serial.println(gps.my_course);
  Serial.print("HDOP: ");Serial.println(gps.my_hdop);
  Serial.print("PDOP: ");Serial.println(gps.my_pdop);
  Serial.print("VDOP: ");Serial.println(gps.my_vdop);
  Serial.print("Deviation: ");Serial.println(gps.my_deviation);
  Serial.print("Tracking: ");Serial.println(gps.num_sats);
  Serial.println();//this should be good for now
  return true;
}

bool cmdsReg(char *buffer)
{
  if (!chk(buffer,"reg")){return false;}
  char *arg1 = next(buffer);
  if (chk(arg1,"help"))
  {
	Serial.println("---------------------------");
	Serial.println("-----Register Commands-----");
	Serial.println("---------------------------");
	Serial.println("--Help menu:");
	Serial.println();
	Serial.println("ls ---- List all the currently registered commands");
  }
  else if (chk(arg1,"ls"))
  {
	for (int i=0;i<coms.registered.size();i++)
	{
	  Serial.print("\t");
	  Serial.print(i);
	  Serial.print(" --> ");
	  Serial.print(coms.registered[i]);
	}
  }
  return true;
}

int charToChannel(char *buffer)
{
  int ch = -1;
  if (chk(buffer,"roll")){ch=1;}
  else if (chk(buffer,"pitch")){ch=2;}
  else if (isThrottle(buffer)){ch=3;}
  else if (chk(buffer,"rudder") || chk(buffer,"rud")){ch=4;}
  else {ch = parseNumberArg(buffer);}
  if (ch == 0 || ch > 4){return -1;}
  return ch;
}

bool isThrottle(char *buffer)
{
  return chk(buffer,"th") || chk(buffer,"thr") || chk(buffer,"thro") || chk(buffer,"throttle");
}

void dispEP(int ch)
{
  Serial.print("CH");
  Serial.print(ch);
  Serial.print(" : ");
  Serial.print(aircraft.getSvoMin(ch));
  Serial.print(" -> ");
  Serial.print(aircraft.getSvoMax(ch));
  if (aircraft.svoGetInvert(ch))
  {
	Serial.print(" Inverted");
  }
  Serial.println();
}

void dispPid(const char *name, void *tpid)
{
  PID &pid = *((PID*)tpid);
  Serial.print(name);
  Serial.print("-- P = ");
  Serial.print(pid.getPGain(),4);
  Serial.print(", I = ");
  Serial.print(pid.getIGain(),4);
  Serial.print(", D = ");
  Serial.print(pid.getDGain(),4);
  if (pid.isLimitMinSet())
  {
	Serial.print(", Min = ");
	Serial.print(pid.getLimitMin());
  }
  if (pid.isLimitMaxSet())
  {
	Serial.print(", Max = ");
	Serial.print(pid.getLimitMax());
  }
  if (pid.isInverted()){Serial.print(", Inverted");}
  Serial.println();
}

bool chk(char *buffer, const char *with)
{
  int i=0;
  while (with[i]!='\0')
  {
	if (buffer[i]!=with[i]){return false;}
	i++;
  }
  return blank(buffer[i]);
}

char* next(char *buffer)
{
  int pos;
  for (pos=0;!blank(buffer[pos]);pos++);
  while (buffer[pos] == ' ' || buffer[pos] == '\t'){pos++;}
  return &buffer[pos];
}

bool blank(char c)
{
  return (c == ' ' || c == '\t' || c == '\n' || c == '\r' || c == '\0');
}

int parseNumberArg(char *buffer)
{
  int num=0,pos;
  for (pos = 0;int(buffer[pos])>=int('0') && int(buffer[pos])<=int('9');pos++)
  {
	num=(num*10)+(int(buffer[pos])-int('0'));
  }
  if (blank(buffer[pos]))
  {
	return num;
  }
  return -1;//error!
}
double parseDoubleArg(const char *buffer)
{
  int i=0,rpos=0;
  double val;
  bool neg = false;
  bool left = true;
  if (buffer[0]=='-'){neg = true;i++;}
  for (;!blank(buffer[i]);i++)
  {
	if (buffer[i]=='.'){left=false;}
	else if (left)
	{
	  val=(val*10)+(buffer[i]-int('0'));
	}
	else
	{
	  rpos++;
	  val += (buffer[i]-int('0'))/pow(10,rpos);
	}
  }
  if (neg){val*=-1;}
  return val;
}

char inhex(int c)
{
  if (c<=9){return char(int('0')+c);}
  return char(int('A')-10+c);
}